using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using System.IO;

namespace RoboSmartIRobotCreate
{
    public enum RobotMode
    {
        Passive,
        Safe,
        Full
    };

    public enum Direction
	{
	    Left,
        Right
	};

    public enum RobotDemo
    {
        Abort,
        Cover,
        CoverAndDock,
        SpotCover,
        Mouse,
        DriveFigure8,
        Wimp,
        Home, 
        Tag,
        PachelBel,
        Banjo
    };

    public enum SensorPacket
    {
        PacketPack0,
        PacketPack1,
        PacketPack2,
        PacketPack3,
        PacketPack4,
        PacketPack5,
        PacketPack6,
        BumpsAndWheelDrops, // 7
        Wall, // 8
        CliffLeft, // 9
        CliffFrotLeft, // 10
        /// <summary>
        /// The state of the cliff sensor on the front right
        /// is sent as 1 bit value, 0 = no cliff, 1 = cliff
        /// </summary>
        CliffFrontRight, //11
        CliffRight, // 12
        VirtualWall, // 13
        LowSideDriverAndWheelOverCurrents, // 14
        /// <summary>
        /// Two unused bytes (Packets 15 and 16) are sent
        /// after the overcurrent byte when the requested packet is 0, 1 or 6.
        /// The value of this two unused bytes is allways 0.
        /// </summary>
        UnusedBytes1, // 15
        /// <summary>
        /// Two unused bytes (Packets 15 and 16) are sent
        /// after the overcurrent byte when the requested packet is 0, 1 or 6.
        /// The value of this two unused bytes is allways 0.
        /// </summary>
        UnusedBytes2, // 16
        InfraredByte, //17
        Buttons,    // 18
        Distance, // 19
        Angle, // 20
        ChargingState, //21
        Voltage, // 22
        Current, //23
        BatteryTemperature, // 24
        BatteryCharge, // 25
        BatteryCapacity, // 26
        WallSignal, // 27
        CliffLeftSignal, //28
        CliffFrontLeftSignal, //29
        CliffFrontRightSignal, // 30
        CliffRightSignal, //31
        CargoBayDigitalInputs, //32
        CargoByAnalogSignal, //33
        ChargingSourcesAvailable, //34
        /// <summary>
        /// Open interface mode is a byte in the range 0-3
        /// as follows: 0 - Off, 1 - Passive, 2 - Safe and 3- Full
        /// </summary>
        OIMode, //35
        SongNumber, //36
        SongPlaying, //37
        NumberOfStreamPackets, //38
        RequestedVelocity, // 39
        RequestedRadius, // 40
        RequestedRightVelocity, //41
        RequestedLeftVelocity // 42
    };

    public enum TouchSensor
    {
        BumpsAndWheelDrops, // 7
        Wall, // 8
        CliffLeft, // 9
        CliffFrotLeft, // 10
        /// <summary>
        /// The state of the cliff sensor on the front right
        /// is sent as 1 bit value, 0 = no cliff, 1 = cliff
        /// </summary>
        CliffFrontRight, //11
        CliffRight, // 12
    }

    public class Robot
    {

        private Connection connection;
        private RobotMode mode;

        public RobotMode Mode
        {
            get 
            {
                return mode; 
            }
            set 
            {
                mode = value; 
            }
        }
	

        public Connection Connection
        {
            get { return connection; }
            set { connection = value; }
        }

        private int batteryCharge;

        public int BatteryCharge
        {
            get 
            {
                GetBatteryStatus();
                return batteryCharge;
            }
        }
	

        public Robot()
        {
            this.connection = new Connection();
        }

        /// <summary>
        /// Constructor that initializes the default
        /// settings for the connection, as specified in
        /// IRobot Create Open Interface Manual
        /// </summary>
        /// <param name="defaultSettings"></param>
        public Robot(bool defaultSettings)
        {
            connection = new Connection();
            connection.BaudRate = 57600;
            connection.DataBits = 8;
            connection.Parity = Parity.None;
            connection.StopBits = StopBits.One;
            
        }

        public void SendBytes(byte[] bytes)
        {
            connection.WriteBytes(bytes);
        }

        #region QUERY SENSORS
        
        /// <summary>
        /// Queries the OI to get the status of all sensors.
        /// Returns an array of 52 bytes, being the grouped information
        /// from all the sensors
        /// </summary>
        /// <returns>Array of 52 bytes, containing results of polling all the sensors</returns>
        public byte[] QueryAllSensors(SensorPacket sensorPacket)
        {
            byte[] result = new byte[52];
            connection.WriteBytes(new byte[] {142,  (byte)sensorPacket});
            if (connection.ReadBytes(result) != 52)
            {
                // should this be here?
                throw new IOException("Could not read the entire result at once");
            }
            return result;
        }

        /// <summary>
        /// Returns an array of bytes, as result of polling the sensors in packet 1
        /// </summary>
        /// <returns></returns>
        public byte[] QuerySensorsPacket1()
        {
            byte[] result = new byte[10];
            // request pack 1
            connection.WriteBytes(new byte[] { 142, 1 });
            // will have to check if all requested bytes were read
            connection.ReadBytes(result);
            return result;
        }

        public int PollTouchSensors()
        {
            byte[] pack1 = QuerySensorsPacket1();
            int sum = 0;
            for (int i = 0; i < 6; i++)
            {
                sum += pack1[i];
            }
            return sum;
        }

        public byte PollTouchSensor(TouchSensor sensor)
        {
            
            connection.WriteBytes(new byte[] { 142, (byte)((byte)sensor + 7) });
            return (byte)connection.ReadByte();
        }

        public int PollLightSensor()
        {
            // send query to analogic input
            connection.WriteBytes(new byte[] {142, 33});
            // must return 2 bytes
            byte bHigh = (byte)connection.ReadByte();
            byte bLow = (byte)connection.ReadByte();
            StreamWriter sw = new StreamWriter(@"e:\outrobot.txt", true);
            int result = 0;
            result = bHigh;
            result <<= 8;
            result |= bLow;
            sw.WriteLine(string.Format("hi: {0} lo: {1} res: {2}", bHigh.ToString("X"), bLow.ToString("X"), result));
            
            sw.Close();
            return result;
        }

        /// <summary>
        /// Gets specific packet information given a byte array
        /// </summary>
        /// <returns></returns>
        public int QuerySensorFromPacket6(SensorPacket packetId, byte[] byteData)
        {
            Int16 result = 0;
            switch (packetId)
            {
                case SensorPacket.PacketPack0:
                    throw new ArgumentException("Please use simple sensor packets!");
                case SensorPacket.PacketPack1:
                    throw new ArgumentException("Please use simple sensor packets!");
                case SensorPacket.PacketPack2:
                    throw new ArgumentException("Please use simple sensor packets!");
                case SensorPacket.PacketPack3:
                    throw new ArgumentException("Please use simple sensor packets!");
                case SensorPacket.PacketPack4:
                    throw new ArgumentException("Please use simple sensor packets!");
                case SensorPacket.PacketPack5:
                    throw new ArgumentException("Please use simple sensor packets!");
                case SensorPacket.PacketPack6:
                    throw new ArgumentException("Please use simple sensor packets!");
                case SensorPacket.BumpsAndWheelDrops:
                    return byteData[0];
                case SensorPacket.Wall:
                    return byteData[1];
                case SensorPacket.CliffLeft:
                    return byteData[2];
                case SensorPacket.CliffFrotLeft:
                    return byteData[3];
                case SensorPacket.CliffFrontRight:
                    return byteData[4];
                case SensorPacket.CliffRight:
                    return byteData[5];
                case SensorPacket.VirtualWall:
                    return byteData[6];
                case SensorPacket.LowSideDriverAndWheelOverCurrents:
                    return byteData[7];
                case SensorPacket.UnusedBytes1:
                    return byteData[8];
                case SensorPacket.UnusedBytes2:
                    return byteData[9];
                case SensorPacket.InfraredByte:
                    return byteData[10];
                case SensorPacket.Buttons:
                    return byteData[11];
                case SensorPacket.Distance:
                    result |= (Int16)byteData[12];
                    result <<= 8;
                    result |= (Int16)byteData[13];
                    return result;
                case SensorPacket.Angle:
                    result |= (Int16)byteData[14];
                    result <<= 8;
                    result |= (Int16)byteData[15];
                    return result;
                case SensorPacket.ChargingState:
                    return byteData[16];
                case SensorPacket.Voltage:
                    result |= (Int16)byteData[17];
                    result <<= 8;
                    result |= (Int16)byteData[18];
                    return result;
                case SensorPacket.Current:
                    return 0 ; // TODO
                case SensorPacket.BatteryTemperature:
                    return 0; // TODO
                case SensorPacket.BatteryCharge:
                    return 0; // TODO
                case SensorPacket.BatteryCapacity:
                    return 0; // TODO
                case SensorPacket.WallSignal:
                    return 0; // TODO
                case SensorPacket.CliffLeftSignal:
                    return 0; // TODO
                case SensorPacket.CliffFrontLeftSignal:
                    return 0; // TODO
                case SensorPacket.CliffFrontRightSignal:
                    break;
                case SensorPacket.CliffRightSignal:
                    break;
                case SensorPacket.CargoBayDigitalInputs:
                    break;
                case SensorPacket.CargoByAnalogSignal:
                    break;
                case SensorPacket.ChargingSourcesAvailable:
                    break;
                case SensorPacket.OIMode:
                    break;
                case SensorPacket.SongNumber:
                    break;
                case SensorPacket.SongPlaying:
                    break;
                case SensorPacket.NumberOfStreamPackets:
                    break;
                case SensorPacket.RequestedVelocity:
                    break;
                case SensorPacket.RequestedRadius:
                    break;
                case SensorPacket.RequestedRightVelocity:
                    break;
                case SensorPacket.RequestedLeftVelocity:
                    break;
                default:
                    throw new ArgumentException("Invalid packet Id");
            }
            return 0;
        }

        #endregion

        #region ROBOT COMMANDS

        /// <summary>
        /// Starts the open interface
        /// </summary>
        /// <param name="mode"></param>
        public void Start(RobotMode mode)
        {
            connection.WriteBytes(new byte[] { 128 });
            byte byteMode = 128;
            if (mode == RobotMode.Full)
            {
                byteMode = 132;
            }

            if (mode == RobotMode.Safe)
            {
                byteMode = 131;
            }
            connection.WriteBytes(new byte[] { byteMode });
        }

        public void Stop()
        {
            connection.WriteBytes(new byte[] { 137, 0, 0, 0, 0 });
        }

        public void Demo(RobotDemo demo)
        {
           connection.WriteBytes(new byte[] {136, (byte)((byte)demo - 1)});
        }

        public void SendExecuteScript()
        {
            connection.WriteBytes(new byte[] { 153 });
        }

        public void ExecuteScript(byte[] script)
        {
            connection.WriteBytes(script);
        }

        private void GetBatteryStatus()
        {
            batteryCharge = 0;
            int batteryTotal = 0;
            byte [] result = new byte[2];
            // read battery charge;
            // will receive two bytes
            connection.WriteBytes(new byte[] { 149, 1, 25 });
            connection.ReadBytes(result);
            
            batteryCharge = (int)((uint)result[1] * 255 + (uint)result[0]);
            connection.WriteBytes(new byte[] { 149, 2, 25 });
            connection.ReadBytes(result);
            batteryTotal = (int)((uint)result[1] * 255 + (uint)result[0]);

            batteryCharge = (100 * batteryCharge) / batteryTotal;

        }

        /// <summary>
        /// Rotates the robot with specified angle
        /// </summary>
        /// <param name="angle">integer value, between 0 and 360</param>
        void Rotate(int angle)
        {
            if ((angle >= 0) && (angle < 360))
            {
                angle = angle % 360;
            }
            // do stuff; convert angle

        }

        /// <summary>
        /// Rotates in place the robot, with 90 degrees 
        /// </summary>
        /// <param name="direction"></param>
        public void Rotate90Degrees(Direction direction)
        {
            // by default roatate counterclockwise
            byte[] command = new byte[] {152, 13, 137, 0, 50, 0, 1, 157, 0, 90, 137, 0, 0, 0, 0, };

            if (direction == Direction.Left)
            {
                // everything is fine
            }
            else
            {
                command[5] = 0xFF;
                command[6] = 0xB0;
                command[8] = 255;
                command[9] = 166;
            }
            connection.WriteBytes(command);
            //execute commad
            connection.WriteBytes(new byte[] { 153 });
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="distance"></param>
        public void DriveStraight(int velocity, int distance)
        {
            // both positive or negative
            if ((distance & 0x8000) == (velocity & 0x8000))
            {
                DriveRobot(velocity, 0x8000, distance);
            }

        }


        // opcode 137
        /// <summary>
        /// 
        /// </summary>
        /// <remarks>Remains to handle if distance we wait for</remarks>
        /// <param name="velocity"></param>
        /// <param name="radius"></param>
        public void DriveRobot(int velocity, int radius, int distance)
        {
            byte bVelocityHigh = (byte) ((velocity & 0xFF00) >> 8);
            byte bVelocityLow =  (byte) (velocity & 0xFF);

            byte bDistanceHigh = (byte)((distance & 0xFF00) >> 8);
            byte bDistanceLow = (byte)(distance & 0xFF);

            byte bRadiusHigh = (byte) ((radius & 0xFF00) >> 8);
            byte bRadiusLow = (byte) (radius & 0xFF);

            // create a script to wait for distance, and to 
            connection.WriteBytes(new byte[] {
                152, 13,   // script; script bytes
                137, bVelocityHigh, bVelocityLow,  bRadiusHigh, bRadiusLow, // drive
                156, bDistanceHigh, bDistanceLow, // wait for distance
                137, 0, 0, 0, 0}); // stop
            connection.WriteBytes(new byte[] {153});
        }

        #endregion
    }
}
